{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "*********************final output of perception agent*********************\n",
      "Perception Prompts:\n",
      "*****Perception Results:*****\n",
      "Front object detections:\n",
      "Front object detected, object type: car, object id: 2, position: (4.36, 9.56), size: (1.86, 4.72)\n",
      "Front object detected, object type: car, object id: 3, position: (-3.70, 13.08), size: (2.01, 4.92)\n",
      "\n",
      "Future trajectories for specific objects:\n",
      "Object type: car, object id: 2, future waypoint coordinates in 3s: [(4.36, 9.56), (4.36, 9.56), (4.36, 9.57), (4.36, 9.57), (4.36, 9.56), (4.36, 9.56)]\n",
      "Object type: car, object id: 3, future waypoint coordinates in 3s: [(-2.66, 13.82), (-1.69, 14.79), (-0.99, 16.13), (-0.25, 17.73), (0.19, 19.42), (0.57, 21.35)]\n",
      "\n",
      "Distance to both sides of road shoulders of current ego-vehicle location:\n",
      "Current ego-vehicle's distance to left shoulder is 7.5m and right shoulder is 4.0m\n",
      "\n",
      "\n",
      "\n",
      "**A Language Agent for Autonomous Driving**\n",
      "Role: You are the brain of an autonomous vehicle (a.k.a. ego-vehicle). In this step, you need to retrieve the most similar past driving experience to help decision-making.\n",
      "\n",
      "Task\n",
      "- You will receive the current driving scenario.\n",
      "- You will also receive several past driving experiences.\n",
      "- You should decide ONLY ONE experience that is most similar to the current scenario based on the information provided.\n",
      "- Please answer ONLY the index (e.g., 0, 1, 2) of the most similar experience.\n",
      "\n",
      "** Current Scenario: **:\n",
      "*****Ego States:*****\n",
      "Current State:\n",
      " - Velocity (vx,vy): (-0.01,0.92)\n",
      " - Heading Angular Velocity (v_yaw): (0.00)\n",
      " - Acceleration (ax,ay): (-0.00,-0.50)\n",
      " - Can Bus: (-0.74,0.14)\n",
      " - Heading Speed: (0.95)\n",
      " - Steering: (-0.02)\n",
      "Historical Trajectory (last 2 seconds): [(-0.07,-6.43), (-0.05,-4.34), (-0.02,-2.32), (-0.01,-0.91)]\n",
      "Mission Goal: FORWARD\n",
      "*****Perception Results:*****\n",
      "Front object detections:\n",
      "Front object detected, object type: car, object id: 2, position: (4.36, 9.56), size: (1.86, 4.72)\n",
      "Front object detected, object type: car, object id: 3, position: (-3.70, 13.08), size: (2.01, 4.92)\n",
      "\n",
      "Future trajectories for specific objects:\n",
      "Object type: car, object id: 2, future waypoint coordinates in 3s: [(4.36, 9.56), (4.36, 9.56), (4.36, 9.57), (4.36, 9.57), (4.36, 9.56), (4.36, 9.56)]\n",
      "Object type: car, object id: 3, future waypoint coordinates in 3s: [(-2.66, 13.82), (-1.69, 14.79), (-0.99, 16.13), (-0.25, 17.73), (0.19, 19.42), (0.57, 21.35)]\n",
      "\n",
      "Distance to both sides of road shoulders of current ego-vehicle location:\n",
      "Current ego-vehicle's distance to left shoulder is 7.5m and right shoulder is 4.0m\n",
      "\n",
      "Found 3 relevant experiences:\n",
      "** Past Driving Experience 1: **\n",
      "*****Past Ego States:*****\n",
      "Current State:\n",
      " - Velocity (vx,vy): (0.00,1.07)\n",
      " - Heading Angular Velocity (v_yaw): (-0.00)\n",
      " - Acceleration (ax,ay): (-0.02,-0.43)\n",
      " - Can Bus: (-0.67,0.03)\n",
      " - Heading Speed: (1.00)\n",
      " - Steering: (0.13)\n",
      "Historical Trajectory (last 2 seconds): [(-0.16,-6.66), (-0.08,-4.46), (-0.03,-2.55), (-0.00,-1.06)]\n",
      "Mission Goal: FORWARD\n",
      "Future trajectories for specific objects:\n",
      "Object type: car, object id: 2, future waypoint coordinates in 3s: [(-1.13, -13.82), (-1.09, -12.18), (-1.05, -10.66), (-0.98, -9.22), (-0.98, -7.96), (-0.93, -6.74)]\n",
      "Object type: car, object id: 3, future waypoint coordinates in 3s: [(-25.19, -17.79), (-25.19, -17.79), (-25.18, -17.78), (-25.18, -17.78), (-25.18, -17.78), (-25.17, -17.78)]\n",
      "** Past Driving Experience 2: **\n",
      "*****Past Ego States:*****\n",
      "Current State:\n",
      " - Velocity (vx,vy): (-0.01,0.97)\n",
      " - Heading Angular Velocity (v_yaw): (0.00)\n",
      " - Acceleration (ax,ay): (-0.01,-0.46)\n",
      " - Can Bus: (-0.68,0.11)\n",
      " - Heading Speed: (1.16)\n",
      " - Steering: (0.04)\n",
      "Historical Trajectory (last 2 seconds): [(-0.06,-6.03), (-0.04,-4.10), (-0.02,-2.40), (-0.01,-0.97)]\n",
      "Mission Goal: FORWARD\n",
      "Front object detections:\n",
      "Front object detected, object type: car, object id: 0, position: (-0.58, 9.54), size: (1.75, 3.99)\n",
      "Front object detected, object type: car, object id: 2, position: (3.57, 7.27), size: (1.88, 4.49)\n",
      "Front object detected, object type: truck, object id: 3, position: (-0.08, 19.97), size: (2.93, 9.04)\n",
      "Future trajectories for specific objects:\n",
      "Object type: car, object id: 2, future waypoint coordinates in 3s: [(3.59, 12.47), (3.61, 17.78), (3.68, 23.26), (3.70, 28.79), (3.66, 34.46), (3.65, 40.07)]\n",
      "Object type: truck, object id: 3, future waypoint coordinates in 3s: [(-0.08, 19.96), (-0.08, 19.93), (-0.09, 19.93), (-0.09, 19.92), (-0.08, 19.92), (-0.09, 19.91)]\n",
      "** Past Driving Experience 3: **\n",
      "*****Past Ego States:*****\n",
      "Current State:\n",
      " - Velocity (vx,vy): (-0.01,1.06)\n",
      " - Heading Angular Velocity (v_yaw): (-0.00)\n",
      " - Acceleration (ax,ay): (0.00,-0.32)\n",
      " - Can Bus: (-0.77,0.04)\n",
      " - Heading Speed: (1.06)\n",
      " - Steering: (-0.01)\n",
      "Historical Trajectory (last 2 seconds): [(-0.08,-6.01), (-0.05,-4.12), (-0.04,-2.43), (-0.02,-1.06)]\n",
      "Mission Goal: FORWARD\n",
      "Front object detections:\n",
      "Front object detected, object type: car, object id: 1, position: (-3.08, 2.56), size: (1.88, 4.52)\n",
      "Future trajectories for specific objects:\n",
      "Object type: car, object id: 2, future waypoint coordinates in 3s: [(-18.54, -24.30), (-18.54, -24.30), (-18.54, -24.30), (-18.54, -24.30), (-18.54, -24.31), (-18.54, -24.31)]\n",
      "Object type: car, object id: 3, future waypoint coordinates in 3s: [(-3.26, -31.78), (-3.43, -28.27), (-3.36, -25.04), (-3.45, -22.14), (-3.46, -19.62), (-3.51, -17.45)]\n",
      "Please return the index 1-3 of the most similar experience: \n",
      "Memory-GPT response: 2\n",
      "*********************final output of memory agent*********************\n",
      "Commonsense Memory:\n",
      "\n",
      "*****Traffic Rules:*****\n",
      "- Avoid collision with other objects.\n",
      "- Always drive on drivable regions.\n",
      "- Avoid driving on occupied regions.\n",
      "- Pay attention to your ego-states and historical trajectory when planning.\n",
      "- Maintain a safe distance from the objects in front of you.\n",
      "\n",
      "\n",
      "*****Past Driving Experience for Reference:*****\n",
      "Most similar driving experience from memory with confidence score: 0.75:\n",
      "The planned trajectory in this experience for your reference:\n",
      "[(0.01,0.98), (0.01,1.87), (0.02,2.40), (0.02,2.64), (0.02,2.76), (0.02,2.86)]\n",
      "\n"
     ]
    }
   ],
   "source": [
    "import sys\n",
    "sys.path.append(\"../..\")\n",
    "from agentdriver.memory.memory_agent import MemoryAgent\n",
    "from agentdriver.perception.perception_agent import PerceptionAgent\n",
    "from agentdriver.llm_core.api_keys import OPENAI_ORG, OPENAI_API_KEY\n",
    "\n",
    "import openai\n",
    "openai.organization = OPENAI_ORG\n",
    "openai.api_key = OPENAI_API_KEY\n",
    "\n",
    "from pathlib import Path\n",
    "\n",
    "data_path = Path('../../data/')\n",
    "\n",
    "perception_agent = PerceptionAgent(token=\"0a0d6b8c2e884134a3b48df43d54c36a\", split=\"val\", data_path=data_path, verbose=False)\n",
    "ego_prompts, perception_prompts, working_memory = perception_agent.run()\n",
    "\n",
    "print(\"*********************final output of perception agent*********************\")\n",
    "print(\"Perception Prompts:\")\n",
    "print(perception_prompts)\n",
    "\n",
    "memory_agent = MemoryAgent(data_path=data_path, verbose=True, compare_perception=True)\n",
    "commonsense_mem, experience_mem = memory_agent.run(working_memory)\n",
    "\n",
    "\n",
    "print(\"*********************final output of memory agent*********************\")\n",
    "print(\"Commonsense Memory:\")\n",
    "print(commonsense_mem)\n",
    "print()\n",
    "print(experience_mem)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "llmagent",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
